package com.poseidon.prototype;

// OPS

import PackagePoseidon.DirectionData;
import PackagePoseidon.DirectionDataSubscriber;

import PackagePoseidon.SpeedData;
import PackagePoseidon.SpeedDataPublisher;

import PackagePoseidon.AngulationData;
import PackagePoseidon.AngulationDataPublisher;

import PackagePoseidon.DistanceData;
import PackagePoseidon.DistanceDataPublisher;

import PackagePoseidon.EnvironmentData;
import PackagePoseidon.EnvironmentDataPublisher;

import PackagePoseidon.ForceData;
import PackagePoseidon.ForceDataPublisher;

import PackagePoseidon.RobotData;
import PackagePoseidon.RobotDataPublisher;

import ops.Participant;
import ops.Topic;
import java.util.Observable;
import java.util.Observer;
import com.jme.math.Vector3f;
import java.util.Vector;
import java.util.HashSet;
import com.jme.math.Quaternion;


public class OPSInterface {

    Topic directionTopic;
    Topic speedTopic;
    Topic angulationTopic;
    Topic distanceTopic;
    Topic environmentTopic;
    Topic forceTopic;
    Topic robotTopic;

    DirectionDataSubscriber subDirection;
    SpeedDataPublisher pubSpeed;
    AngulationDataPublisher pubAngulation;
    DistanceDataPublisher pubDistance;
    EnvironmentDataPublisher pubEnvironment;
    ForceDataPublisher pubForce;
    RobotDataPublisher pubRobot;

    Physics physics;



    public OPSInterface(Physics physics) {
        this.physics = physics;
        this.initOPS();
    }

    
    public void initOPS() {

        Participant participant = Participant.getInstance("PoseidonDomain");
        participant.addTypeSupport(new OPSIDLPoseidonFinal.OPSIDLPoseidonFinalTypeFactory());

        directionTopic = participant.createTopic("DirectionTopic");
        speedTopic = participant.createTopic("SpeedTopic");
        angulationTopic = participant.createTopic("AngulationTopic");
        distanceTopic = participant.createTopic("DistanceTopic");
        environmentTopic = participant.createTopic("EnvironmentTopic");
        forceTopic = participant.createTopic("ForceTopic");
        robotTopic = participant.createTopic("RobotTopic");

        subDirection = new DirectionDataSubscriber(directionTopic);
        pubSpeed = new SpeedDataPublisher(speedTopic);
        pubSpeed.setName("PublisherSpeed");
        pubAngulation = new AngulationDataPublisher(angulationTopic);
        pubAngulation.setName("PublisherAngulation");
        pubDistance = new DistanceDataPublisher(distanceTopic);
        pubDistance.setName("PublisherDistance");
        pubEnvironment = new EnvironmentDataPublisher(environmentTopic);
        pubEnvironment.setName("PublisherEnvironment");
        pubForce = new ForceDataPublisher(forceTopic);
        pubForce.setName("PublisherForce");
        pubRobot = new RobotDataPublisher(robotTopic);
        pubRobot.setName("PublisherRobot");
        
    }

    public void addDirectionObserverOPS(Simulator simulator) {
        subDirection.addObserver(simulator);
        subDirection.start();
    }



    public void sendPhysicsInfo() {

        //Environment

        String[] envObj = new String[3];
        envObj[0] = physics.getWaterDensity();
        envObj[1] = physics.getFrictionAreaReference();
        envObj[2] = physics.getFrictionDragCoefficient();


        
        //Force
        
        String[] forObj = new String[4];
        forObj[0] = Double.toString(physics.getLiftPushUp());
        forObj[1] = Double.toString(physics.getGravityPushDown());
        forObj[2] = Double.toString(physics.getImpulsePush());
        forObj[3] = Double.toString(physics.getFrictionPush());
        



        //Robot

        String[] robObj = new String[11];
        robObj[0] = physics.getRobotMaxSpeed();
        robObj[1] = physics.getRobotMass();
        robObj[2] = physics.getRobotLenght();
        robObj[3] = physics.getRobotHeight();
        robObj[4] = physics.getRobotPayload();
        robObj[5] = Double.toString(physics.getRobotThrustForwardForce());
        robObj[6] = Double.toString(physics.getRobotThrustLateralForce());
        robObj[7] = Double.toString(physics.getRobotThrustVerticalForce());
        robObj[8] = physics.getMaximumWorkingDepth();
        robObj[9] = Double.toString(physics.getRobotVolume());
        robObj[10] = Double.toString(physics.getRobotDensity());
        


        writeInfoOPS("Environment", envObj);
        writeInfoOPS("Force", forObj);
        writeInfoOPS("Robot", robObj);


        System.out.println("Environment: " + envObj.toString());
        System.out.println("Force: " + forObj.toString());
        System.out.println("Robot: " + robObj.toString());
        System.out.println("");
    }

    public void writeInfoOPS(String topic, Object information) {

        if (topic.equals("Speed")) {
            String[] speObj = (String[]) information;

            SpeedData dataSpe = new SpeedData();
            dataSpe.xSpeed = speObj[0];
            dataSpe.ySpeed = speObj[1];
            dataSpe.zSpeed = speObj[2];
            
            pubSpeed.write(dataSpe);
        }

        if (topic.equals("Angulation")) {
            String[] angObj = (String[]) information;

            AngulationData dataAng = new AngulationData();
            dataAng.xAngulation = angObj[0];
            dataAng.yAngulation = angObj[1];
            dataAng.zAngulation = angObj[2];

            pubAngulation.write(dataAng);
        }

        if (topic.equals("Distance")) {
            String[] disObj = (String[]) information;

            DistanceData dataDis = new DistanceData();
            dataDis.xDistance = disObj[0];
            dataDis.yDistance = disObj[1];
            dataDis.zDistance = disObj[2];

            pubDistance.write(dataDis);
        }

        if (topic.equals("Environment")) {
            String[] envObj = (String[]) information;

            EnvironmentData dataEnv = new EnvironmentData();
            dataEnv.waterDensity = envObj[0];
            dataEnv.frictionAreaReference = envObj[1];
            dataEnv.frictionDragCoefficient = envObj[2];

            pubEnvironment.write(dataEnv);
        }

        if (topic.equals("Force")) {
            String[] forObj = (String[]) information;

            ForceData dataFor = new ForceData();
            dataFor.liftPushUp = forObj[0];
            dataFor.gravityPushDown = forObj[1];
            dataFor.impulsePush = forObj[2];
            dataFor.frictionPush = forObj[3];

            pubForce.write(dataFor);
        }

        if (topic.equals("Robot")) {
            String[] robObj = (String[]) information;

            RobotData dataRob = new RobotData();
            dataRob.robotMaxSpeed = robObj[0];
            dataRob.robotMass = robObj[1];
            dataRob.robotLenght = robObj[2];
            dataRob.robotHeight = robObj[3];
            dataRob.robotPayload = robObj[4];
            dataRob.robotThrustForwardForce = robObj[5];
            dataRob.robotThrustLateralForce = robObj[6];
            dataRob.robotThrustVerticalForce = robObj[7];
            dataRob.maximumWorkingDepth = robObj[8];
            dataRob.robotVolume = robObj[9];
            dataRob.robotDensity = robObj[10];

            pubRobot.write(dataRob);
        }
    }

}
